#include "guitab.h"
#include <ros/package.h>


// Basic template classes for conversion between strings and numbers
template<class T>
T stringToNumber(const string &str)
{
    stringstream ss(str);
    T ret;
    ss >> ret;
    return ret;
}

template<class T>
string numberToString(T num)
{
    stringstream ss;
    ss << num;
    return ss.str();
}


cv::Mat Gui::imgPlayButton;
cv::Mat Gui::imgStopButton;
cv::Mat Gui::imgRecordButton;
cv::Mat ListRecordingsTab::imgRemoveButton;

cv::Mat ViewPointsTab::imgNextButton;
cv::Mat ViewPointsTab::imgPrevButton;
cv::Mat ViewPointsTab::imgGripButton;

Gui::Gui(RecordController *recorder) :
    playButton(480, 490, 100, 100),
    stopButton(590, 490, 100, 100),
	recordButton(700, 490, 100, 100),
	saveButton(440,490,35,20),
	loadButton(440,520,35,20)

{
    mRec = NULL;
	mActiveRecording = NULL;
    mActiveTab = 0;

    mRec = recorder;

    // Create tabs here
    mTabs.push_back(new ListRecordingsTab(cv::Rect(10, 10, 90, 30), this));
    mTabs.push_back(new ViewPointsTab(cv::Rect(110, 10, 150, 30), this));
}

void Gui::Init()
{
    // Load images for GUI
    string pkgPath = ros::package::getPath("recorder");
    imgPlayButton = cv::imread(pkgPath + "/data/play.png");
    imgStopButton = cv::imread(pkgPath + "/data/stop.png");
    imgRecordButton = cv::imread(pkgPath + "/data/record.png");
    if(imgPlayButton.data == NULL ||
            imgStopButton.data == NULL ||
            imgRecordButton.data == NULL)
    {
        ROS_WARN("Couldn't load image!");
    }

    ListRecordingsTab::Init();
    ViewPointsTab::Init();
}

bool Gui::HandleInput(int event, cv::Point2d mouse)
{
    for(unsigned int i = 0; i < mTabs.size(); i++)
    {
        bool activate = mTabs[i]->HandleInput(event, mouse, (i == mActiveTab));
        if(activate) mActiveTab = i;
    }

    // Playback functionallity
    if(event == CV_EVENT_LBUTTONDOWN)
    {
        // Check playback controls
        if(playButton.contains(mouse))
        {
            mRec->Play(mActiveRecording);
        }
        else if(stopButton.contains(mouse))
        {
            mRec->mState = RecordController::STANDBY;
        }
        else if(recordButton.contains(mouse))
        {
            mRec->mState = RecordController::RECORDING;
        }

		// Save & Load
		string pkgPath = ros::package::getPath("recorder");

		if(saveButton.contains(mouse) && mActiveRecording)
		{
			ROS_INFO("Saved as [%s]", "some.txt");
			mActiveRecording->Save(pkgPath + "/load/some.txt");
		}
		else if(loadButton.contains(mouse))
		{
			ROS_INFO("Load!");
			Recording* newRec = new Recording;

			try
			{
				newRec->Load(pkgPath + "/load/some.txt");
				mRec->GetRecordings().push_back(newRec);
			}
			catch(string s)
			{
				ROS_ERROR("Failed to load file %s!", "/load/some.txt");
				delete newRec;
			}
		}
    }

    return false;
}

void Gui::Draw(cv::Mat target)
{
    // Draw tabs
    for(unsigned int i = 0; i < mTabs.size(); i++)
        mTabs[i]->Draw(target, i == mActiveTab);

    // ---------- //
    // Status bar //
    // ---------- //

    cv::Rect statusRect(20, 500, 400, 80);
    cv::rectangle(target, statusRect, cv::Scalar(0,0,0));

    // Show data about current recording
    if(mActiveRecording)
    {
        cv::Point2d statusTextPos(30, 520);
        string currentTime;
        if(mRec->mPlaybackInterface)
            currentTime = numberToString(mRec->mPlaybackInterface->GetPlaybackTimer().toSec());
        else
            currentTime = "0";
        string maxTime = numberToString(mActiveRecording->GetMaxTime().toSec());

        cv::putText(target, currentTime + "/" + maxTime, statusTextPos, CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));
    }

    // ------------------ //
    // Playback interface //
    // ------------------ //
    cv::Mat playPos = target(playButton);
    cv::Mat stopPos = target(stopButton);
    cv::Mat recordPos = target(recordButton);
    imgPlayButton.copyTo(playPos);
    imgStopButton.copyTo(stopPos);
    imgRecordButton.copyTo(recordPos);

	// Save and load rects
	cv::rectangle(target,saveButton, cv::Scalar(0,0,0));
	cv::rectangle(target,loadButton, cv::Scalar(0,0,0));
	cv::putText(target, "Save", cv::Point2d(saveButton.x + 1, saveButton.y + 10), CV_FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(0,0,0));
	cv::putText(target, "Load", cv::Point2d(loadButton.x + 1, loadButton.y + 10), CV_FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(0,0,0));

    cv::Point2d recorderStatusRect(480, 480);
    // Print the state of the recorder [STANDBY, RECORDING, PLAYBACK]
    cv::putText(target, "Recorder state:", recorderStatusRect, CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));
    if(mRec->mState == RecordController::STANDBY)
        cv::putText(target, "Standby", recorderStatusRect + cv::Point2d(200,0), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,255,0));
    else if(mRec->mState == RecordController::RECORDING)
        cv::putText(target, "Recording...", recorderStatusRect + cv::Point2d(200,0), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,255));
    else if(mRec->mState == RecordController::PLAYBACK)
        cv::putText(target, "Playback", recorderStatusRect + cv::Point2d(200,0), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255,0,0));
    else
        cv::putText(target, "BROKEN (state not recognized)", recorderStatusRect + cv::Point2d(200,0), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,255));
}

GuiTab::GuiTab(cv::Rect tabrect, string text, Gui* parent)
{
    mTabRect = tabrect;
    mText = text;
    mSelectable = true;
    mParent = parent;
	mChosen = 0;
}

bool GuiTab::HandleInput(int event, cv::Point2d mouse, bool active)
{
    return (mSelectable && (event == CV_EVENT_LBUTTONDOWN) && mTabRect.contains(mouse));
}

void GuiTab::Draw(cv::Mat target, bool active)
{
    cv::rectangle(target, mTabRect, cv::Scalar(220,220,220), -1);
    if(active)
    {
        cv::putText(target, mText, cv::Point2d(mTabRect.x + 10, mTabRect.y + 20), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,255,0));
    }
    else if(mSelectable)
    {
        cv::putText(target, mText, cv::Point2d(mTabRect.x + 10, mTabRect.y + 20), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(128,128,128));
    }
    else
    {
        cv::putText(target, mText, cv::Point2d(mTabRect.x + 10, mTabRect.y + 20), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,128));
    }
}

ListRecordingsTab::ListRecordingsTab(cv::Rect tabrect, Gui* parent) :
    GuiTab(tabrect, "List", parent),
    removeButton(0, 0, 20, 20),
	activateArea(0,0,200,20),
	transferButton(0,0,30,30)
{
}

// Only calculate the position of remove buttons in one place
// If we want to move the remove buttons around, just change here
cv::Rect ListRecordingsTab::RemoveButtonPos(int nr, int offSet)
{
    cv::Rect recRemButtonPos = removeButton;
	recRemButtonPos.x += 10 + offSet;
    recRemButtonPos.y += -15 + 200+nr*30;
    return recRemButtonPos;
}

cv::Rect ListRecordingsTab::ActivateAreaPos(int nr, int offSet)
{
    cv::Rect recActivateAreaPos = activateArea;
	recActivateAreaPos.x += 40 + offSet;
	recActivateAreaPos.y += -15 + 200+nr*30;
    return recActivateAreaPos;
}

cv::Rect ListRecordingsTab::TransferButtonPos(int nr, int offSet)
{
	cv::Rect recTransferButtonPos = transferButton;
	recTransferButtonPos.x += 300 + offSet;
	recTransferButtonPos.y += -15 + 200+nr*30;
	return recTransferButtonPos;
}

// Remove? Does it need to do anything more?
void ListRecordingsTab::Init()
{
    string pkgPath = ros::package::getPath("recorder");
    imgRemoveButton = cv::imread(pkgPath + "/data/remove.png");
    if(imgRemoveButton.data == NULL)
        ROS_WARN("Couldn't load image!");
}

bool ListRecordingsTab::HandleInput(int event, cv::Point2d mouse, bool active)
{
    // Check for local buttons
    if(active && (event == CV_EVENT_LBUTTONDOWN))
    {
        vector<Recording*>& recordings = mParent->mRec->GetRecordings();
		vector<Recording*>& passiveRecordings = mParent->mRec->GetPassiveRecordings();

        for(size_t i = 0; i < recordings.size(); ++i)
        {
			cv::Rect activateArea = ActivateAreaPos(i,0);
			cv::Rect transferArea = TransferButtonPos(i, 0);

            // Did we remove the recording?
			cv::Rect recRemButtonPos = RemoveButtonPos(i,0);
            if(recRemButtonPos.contains(mouse))
            {
                Recording* record = recordings[i];
                if(record == mParent->mActiveRecording)
                    mParent->mActiveRecording = NULL;
                if(record != NULL)
                    delete record;
                recordings.erase(recordings.begin()+i);
                break; // Don't delete more than one recording per click
            }
            else if(activateArea.contains(mouse))
                mParent->mActiveRecording = recordings[i];
			else if(transferArea.contains(mouse))
			{
				passiveRecordings.push_back(recordings[i]);
				recordings.erase(recordings.begin()+i);
				break;
			}
        }

		// Passive to active
		int offSet = 460;
		for(size_t i = 0; i < passiveRecordings.size(); ++i)
		{
			cv::Rect activateArea = ActivateAreaPos(i,offSet);
			cv::Rect transferArea = TransferButtonPos(i,offSet-410);

			// Did we remove the recording?
			cv::Rect recRemButtonPos = RemoveButtonPos(i,offSet);
			if(recRemButtonPos.contains(mouse))
			{
				Recording* record = passiveRecordings[i];
				if(record == mParent->mActiveRecording)
					mParent->mActiveRecording = NULL;
				if(record != NULL)
					delete record;
				passiveRecordings.erase(passiveRecordings.begin()+i);
				break; // Don't delete more than one recording per click
			}
			else if(activateArea.contains(mouse))
				mParent->mActiveRecording = passiveRecordings[i];
			else if(transferArea.contains(mouse))
			{
				recordings.push_back(passiveRecordings[i]);
				passiveRecordings.erase(passiveRecordings.begin()+i);
				break;
			}
		}
    }

    return GuiTab::HandleInput(event, mouse, active);
}

void ListRecordingsTab::Draw(cv::Mat target, bool active)
{
    GuiTab::Draw(target, active);

    if(active)
    {
        const vector<Recording*>& recordings = mParent->mRec->GetRecordings();
		const vector<Recording*>& passiveRecordings = mParent->mRec->GetPassiveRecordings();

        // Draw recordings
        cv::putText(target, "Recordings:", cv::Point2d(50, 150), CV_FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(0,0,0));
		cv::putText(target, "Passive recordings:", cv::Point2d(450, 150), CV_FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(0,0,0));

        int index = 0;
		int offSet = 0;
        for(vector<Recording*>::const_iterator it = recordings.begin(); it != recordings.end(); ++it)
        {
            const Recording* r = *it;

			cv::Rect activateArea = ActivateAreaPos(index, offSet);
            if(r == mParent->mActiveRecording)
                cv::rectangle(target, activateArea, cv::Scalar(0,255,0));
            else
                cv::rectangle(target, activateArea, cv::Scalar(0,0,0));

            // TODO: How to visually represent?
            cv::putText(target,
                        "ID: " + numberToString(r->ID()) + " contains " + numberToString(r->numPoses()) + " poses.",
                        cv::Point2d(50, 200+index*30),
                        CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));

			cv::Rect transferArea = TransferButtonPos(index, offSet);
			cv::rectangle(target, transferArea,cv::Scalar(100,100,0));

			cv::Rect recRemButtonPos = RemoveButtonPos(index, offSet);
            cv::Mat removePos = target(recRemButtonPos);
            imgRemoveButton.copyTo(removePos);

            index++;
        }

		// Passive recordings
		index = 0;
		offSet = 460;
		for(vector<Recording*>::const_iterator it = passiveRecordings.begin(); it != passiveRecordings.end(); ++it)
		{
			const Recording* r = *it;

			cv::Rect activateArea = ActivateAreaPos(index, offSet);
			if(r == mParent->mActiveRecording)
				cv::rectangle(target, activateArea, cv::Scalar(0,255,0));
			else
				cv::rectangle(target, activateArea, cv::Scalar(0,0,0));

			// TODO: How to visually represent?
			cv::putText(target,
						"ID: " + numberToString(r->ID()) + " contains " + numberToString(r->numPoses()) + " poses.",
						cv::Point2d(510, 200+index*30),
						CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));

			cv::Rect transferArea = TransferButtonPos(index, offSet - 410);
			cv::rectangle(target, transferArea,cv::Scalar(100,0,100));

			cv::Rect recRemButtonPos = RemoveButtonPos(index, offSet);
			cv::Mat removePos = target(recRemButtonPos);
			imgRemoveButton.copyTo(removePos);

			index++;
		}
    }
}

const int xButton = 600;
const int yButton = 100;

ViewPointsTab::ViewPointsTab(cv::Rect tabrect, Gui* parent) :
    GuiTab(tabrect, "View points", parent),
    nextButton(xButton+55, yButton, 50, 50),
    prevButton(xButton,yButton, 50, 50),
    gripButton(xButton+27, yButton + 50, 50, 50),
    next2Button(xButton+110, yButton, 50, 50),
    prev2Button(xButton-55,yButton, 50, 50)
{
}

void ViewPointsTab::Init()
{
    string pkgPath = ros::package::getPath("recorder");
    imgNextButton = cv::imread(pkgPath + "/data/right.png");
    imgPrevButton = cv::imread(pkgPath + "/data/left.png");
    imgGripButton = cv::imread(pkgPath + "/data/switch.png");
    if(imgNextButton.data == NULL || imgPrevButton.data == NULL || imgGripButton.data == NULL)
        ROS_WARN("Couldn't load image!");
}

cv::Rect gripBox(int i){
    return cv::Rect(121 + 21*i,100,20,20);
}
// Buttons for iterating and changing gripper states of poses
bool ViewPointsTab::HandleInput(int event, cv::Point2d mouse, bool active)
{
    if(active && (event == CV_EVENT_LBUTTONDOWN))
    {
        Recording* curRec = mParent->mActiveRecording;
        if(curRec)
        {
            vector<PoseInternal>& poses = curRec->Poses();

            if(gripButton.contains(mouse)){
                if(poses[mChosen].mGripper == PoseInternal::LIMBO)
                    poses[mChosen].mGripper = PoseInternal::CLOSED;
                else if(poses[mChosen].mGripper == PoseInternal::CLOSED)
                    poses[mChosen].mGripper = PoseInternal::OPEN;
                else
                    poses[mChosen].mGripper = PoseInternal::LIMBO;
            }

            if(prevButton.contains(mouse) && mChosen>0)
                mChosen -= 1;
            else if(nextButton.contains(mouse) && (unsigned)mChosen<poses.size()-1)
                mChosen += 1;
            else if(prev2Button.contains(mouse) && mChosen>0){
                if((unsigned)mChosen < 5)
                    mChosen = 0;
                else
                    mChosen -= 5;
            }
            else if(next2Button.contains(mouse) && (unsigned)mChosen<poses.size()-1){
                if((unsigned)mChosen+5 > poses.size()-1)
                    mChosen = poses.size()-1;
                else
                    mChosen += 5;
            }
        }
    }

    return GuiTab::HandleInput(event, mouse, active);
}

void ViewPointsTab::Draw(cv::Mat target, bool active)
{
    GuiTab::Draw(target, active);

    if(active)
    {
        const PlaybackInterface* playback = mParent->mRec->mPlaybackInterface;
        const double startX = 400;
        const double startY = 300;
        int lastPose = -1;
        if(playback)
            lastPose = playback->LastPose();

        // If the tab is active retrieve the current recording
        Recording* curRec = mParent->mActiveRecording;
        if(curRec)
        {
            vector<PoseInternal>& poses = curRec->Poses();

            double mX = poses[mChosen].mX;
            double mY = poses[mChosen].mY;

            // Load pictures of the buttons
            cv::Mat nextPos = target(nextButton);
            cv::Mat prevPos = target(prevButton);
            cv::Mat next2Pos = target(next2Button);
            cv::Mat prev2Pos = target(prev2Button);
            cv::Mat gripPos = target(gripButton);


            imgNextButton.copyTo(nextPos);
            imgPrevButton.copyTo(prevPos);
            imgGripButton.copyTo(gripPos);

            imgNextButton.copyTo(next2Pos);
            imgPrevButton.copyTo(prev2Pos);

			// Describing text
            cv::putText(target, "Prev", cv::Point2d (prevButton.x+5,prevButton.y-5), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));
            cv::putText(target, "Next", cv::Point2d (nextButton.x+5,nextButton.y-5), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));
            cv::putText(target, "Gripper", cv::Point2d (gripButton.x+50,gripButton.y+25), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));
			cv::putText(target, "Red = Closed  Green = Open", cv::Point2d (gripButton.x-100,gripButton.y+65), CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0,0,0));

            // Mark the first pose
            if(poses.size())
            {
                if(lastPose == -1 || lastPose == 0)
                    cv::circle(target, cv::Point2d(poses[0].mX + startX - mX, poses[0].mY + startY - mY), 4,  cv::Scalar(0,128,0));
                else
                    cv::circle(target, cv::Point2d(poses[0].mX + startX - mX, poses[0].mY + startY - mY), 2,  cv::Scalar(0,0,255));
            }
            // Iterate over all poses in the recording and draw their x,y position, with cirles at each pose and lines between poses
            for(unsigned int i = 0; i < poses.size(); i++)
            {
                cv::Point2d p1(poses[i].mX + startX - mX, poses[i].mY + startY - mY);

                if(i + 1 < poses.size()){
                    cv::Point2d p2(poses[i+1].mX + startX - mX, poses[i+1].mY + startY - mY);
                    cv::line(target, p1, p2, cv::Scalar(0,0,255));

                    if((unsigned int)lastPose == i+1)
                        cv::circle(target, p2, 4, cv::Scalar(0,128,0));
                    else
                        cv::circle(target, p2, 2, cv::Scalar(0,0,255));
                }
                // Marks the chosen pose
                if(i == (unsigned)mChosen)
                    cv::circle(target, p1, 6, cv::Scalar(200,0,200));

                // Marks the gripper state of the chosen pose
                if(poses[i].mGripper == PoseInternal::CLOSED)
                    cv::circle(target, p1, 3, cv::Scalar(0,0,255),-1);
                else if(poses[i].mGripper == PoseInternal::OPEN)
                    cv::circle(target, p1, 3, cv::Scalar(0,255,0),-1);
            }
        }
        else
        {
            // No active recording
        }
    }
}
